#define _USE_MATH_DEFINES
#include <cmath>
#include "AlgoEvaluatorModule14DangerousAreaExposure.h"

// Helper function to calculate distance between two lat/lon points in meters using Haversine formula.
// TODO: Move this to a common utility file to avoid duplication.
static double haversine_distance(double lat1, double lon1, double lat2, double lon2) {
    const double R = 6371000.0; // Earth radius in meters
    auto to_radians = [](double degree) {
        return degree * M_PI / 180.0;
    };

    double lat1_rad = to_radians(lat1);
    double lon1_rad = to_radians(lon1);
    double lat2_rad = to_radians(lat2);
    double lon2_rad = to_radians(lon2);

    double dlat = lat2_rad - lat1_rad;
    double dlon = lon2_rad - lon1_rad;

    double a = sin(dlat / 2.0) * sin(dlat / 2.0) +
               cos(lat1_rad) * cos(lat2_rad) *
               sin(dlon / 2.0) * sin(dlon / 2.0);
    double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));

    return R * c;
}

// Helper function to check if a point is inside a polygon using the Ray Casting algorithm.
static bool is_inside_polygon(const algobase::Coord& point, const algobase::Polygon& polygon)
{
    if (polygon.empty()) {
        return false;
    }
    int intersections = 0;
    for (size_t i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) {
        if (((polygon[i][1] > point[1]) != (polygon[j][1] > point[1])) &&
            (point[0] < (polygon[j][0] - polygon[i][0]) * (point[1] - polygon[i][1]) / (polygon[j][1] - polygon[i][1]) + polygon[i][0])) {
            intersections++;
        }
    }
    return intersections % 2 == 1;
}


class algo::AlgoEvaluatorModule14DangerousAreaExposurePrivate
{
public:
};

algo::AlgoEvaluatorModule14DangerousAreaExposure::AlgoEvaluatorModule14DangerousAreaExposure()
	: d(new AlgoEvaluatorModule14DangerousAreaExposurePrivate)
{
}

algo::AlgoEvaluatorModule14DangerousAreaExposure::~AlgoEvaluatorModule14DangerousAreaExposure()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule14DangerousAreaExposure::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>();
}

int algo::AlgoEvaluatorModule14DangerousAreaExposure::exec(const algobase::Path& path, double& result, algobase::EvaluateModuleCoords& indexCoords)
{
	if (path.size() < 2) {
		result = 0.0;
		indexCoords.clear();
		return 0;
	}

	auto task_config = getTaskConfig();
	const auto& restricted_areas_it = task_config.mTypePolygons.find(algobase::AT_Restricted);
	if (restricted_areas_it == task_config.mTypePolygons.end()) {
		result = 0.0; // No dangerous areas defined.
        //
		indexCoords.resize(path.size());
		for (size_t i = 0; i < path.size(); ++i) {
			indexCoords[i].Assign(path[i]);
			indexCoords[i].mRawValue = 0.0;
			indexCoords[i].mResult = 0.0;
		}
		//dummy data
		return 0;
	}
	const auto& dangerous_polygons = restricted_areas_it->second;

	double total_exposure_value = 0.0;
	indexCoords.resize(path.size());
	indexCoords[0].Assign(path[0]);
	indexCoords[0].mRawValue = 0.0;

	for (size_t i = 0; i < path.size() - 1; ++i) {
		const auto& p1 = path[i];
		const auto& p2 = path[i+1];

		// Simplified model: Check if the midpoint of the segment is in a dangerous area.
		algobase::Coord midpoint = { (p1.mCoordZ[0] + p2.mCoordZ[0]) / 2.0, (p1.mCoordZ[1] + p2.mCoordZ[1]) / 2.0 };
		
		bool in_danger = false;
		for (const auto& poly : dangerous_polygons) {
			if (is_inside_polygon(midpoint, poly)) {
				in_danger = true;
				break;
			}
		}

		double segment_exposure = 0.0;
		if (in_danger) {
			// Calculate segment time
			double h_dist = haversine_distance(p1.mCoordZ[1], p1.mCoordZ[0], p2.mCoordZ[1], p2.mCoordZ[0]);
			double v_dist = std::abs(p2.mCoordZ[2] - p1.mCoordZ[2]);
			double segment_dist = std::sqrt(h_dist * h_dist + v_dist * v_dist);
			double speed_mps = p1.mSpeed;
			double segment_time = (speed_mps > 1e-6) ? (segment_dist / speed_mps) : 0.0;

			// Exposure = Time * RiskLevel (assuming RiskLevel = 1.0 for all restricted areas)
			segment_exposure = segment_time * 1.0;
			total_exposure_value += segment_exposure;
		}
		
		// Store the INSTANTANEOUS exposure for the segment at the start point.
		indexCoords[i].Assign(p1);
		indexCoords[i].mRawValue = segment_exposure;
		indexCoords[i].mResult = calcScore(segment_exposure);
	}
	// Last point has no outgoing segment.
	indexCoords.back().Assign(path.back());
	indexCoords.back().mRawValue = 0.0;

	result = calcScore(total_exposure_value);
	return 0;
}

double algo::AlgoEvaluatorModule14DangerousAreaExposure::calcScore(double rawValue) const
{
	return costSigmoid(rawValue, 0.0, 60.0) * getWeight(); // Penalize if over 1 minute in danger.
}

double algo::AlgoEvaluatorModule14DangerousAreaExposure::calcRaw(const std::vector<double>& rawValues) const
{
	double total_exposure_value = 0.0;
	for (const auto& val : rawValues) {
		total_exposure_value += val;
	}
	return total_exposure_value;
}
